#include "NRF24/nrf24_app.h"
#include "math.h"
#include "stdio.h"
#include "stdlib.h"

/**
 ****************************************************************************************************

//RF2G4_Send_Data[0] 上点动按钮
//RF2G4_Send_Data[1] 下点动按钮
//RF2G4_Send_Data[2] 右点动按钮
//RF2G4_Send_Data[3] 左点动按钮

//RF2G4_Send_Data[4] 左摇杆按钮
//RF2G4_Send_Data[5] 右摇杆按钮

//RF2G4_Send_Data[6] 左自锁按钮
//RF2G4_Send_Data[7] 右自锁按钮

//RF2G4_Send_Data[8] 右电位器值
//RF2G4_Send_Data[9] 左电位器值

//RF2G4_Send_Data[10] 左手摇杆上下
//RF2G4_Send_Data[11] 左手摇杆左右
//RF2G4_Send_Data[12] 右手摇杆上下
//RF2G4_Send_Data[13] 右手摇杆左右


****************************************************************************************************
**/

// 全局变量
//---------------------------------------------------------
uint16_t C_TIM2_IT_Update = 0 ;		// TIM3的定时计次

uint16_t C_2G4_Command = 0 ;			// 2.4G指令计时

uint8_t  F_2G4_Command_Valid = 0 ;	// 2.4G命令有效标志位

#define NRF24_DATA_HUART  Test_huart

uint16_t motor_start_flag = 0;
uint16_t water_measure_flag = 0;
uint16_t watch_dog_flag = 0;
uint16_t motor_stop_flag = 0;
uint16_t GNSS_flag = 0;
uint16_t SENSOR_flag = 0;

ADC_Dev_val ADC_dev_val = {0};
double Water_depth_val;
double Water_sample_val;

#define MOTOR_MAPPING(__val__)  (uint16_t)(fabs(1.0f*__val__) / 127.0 * 100.0)

//自主巡航转向状态位
uint8_t AUTO_MOVE_TURN = 0;
uint8_t AUTO_MOVE_TURN_L = 0;
uint8_t AUTO_MOVE_TURN_R = 0;

bool ADC_DATA_Dispose_status = false;
//---------------------------------------------------------

/**
 * @name 初始化函数
 * @brief 初始化2.4G模块并检测是否正常，配置为接收模式
 */
void NRF24_Init(void)
{
	RF2G4_Init();//2.4射频初始化

	USART_printf(NRF24_DATA_HUART,"NRF INIT OK\r\n");

	if(RF2G4_Check())	//判断是否正常
	{ 
		USART_printf(NRF24_DATA_HUART,"NRF ERROR\r\n");
	}
	else
	{ 
		USART_printf(NRF24_DATA_HUART,"NRF OK\r\n");
	} 

	RF2G4_RX_Mode();//配置为接收模式
}

/**
 * @name 接收处理函数
 * @brief 处理2.4G模块接收的数据
 * @param RF2G4_Receive_Data 接收的数据包，uint8_t数据类型
 */
void RX_Dispose(uint8_t *RF2G4_Receive_Data)
{
	if(RF2G4_Rx_Packet(RF2G4_Receive_Data,14) == RX_SUCCESS)	// 判断是否接收到数据	
	{		
		watch_dog_flag=0;
		motor_stop_flag = 0;
		USART_printf(NRF24_DATA_HUART,"NRF RX OK\r\n");//接收成功
		//-----------------------------------------------
		F_2G4_Command_Valid = 1 ;	// 2.4G指令有效位置1
			
		C_2G4_Command = 0 ;			// 重新对2.4G指令计时
		//-----------------------------------------------	
		// 打印调试信息
        for (uint8_t i = 0; i < 14; i++)
        {
            USART_printf(NRF24_DATA_HUART, "RF2G4_Receive_Data[%d]: %d\r\n", i, RF2G4_Receive_Data[i]);
        }

        // 更新按键状态
        motor_start_flag   = RF2G4_Receive_Data[6]; // R键：启动电机
        water_measure_flag = RF2G4_Receive_Data[7]; // L键：取水样模式
		GNSS_flag = RF2G4_Receive_Data[4]; // 摇杆左键：重置GNSS
		SENSOR_flag = RF2G4_Receive_Data[5]; // 摇杆邮件，重置DS18B20和超声波

		// // 摇杆值提取并转换为有符号数（中心点127）
        uint8_t left_y  = (RF2G4_Receive_Data[10] > JOYSTICK_DEADZONE_UP || RF2G4_Receive_Data[10] < JOYSTICK_DEADZONE_DOWN) ? 1 : 0; // 左摇杆上下
        uint8_t left_x  = (RF2G4_Receive_Data[11] > JOYSTICK_DEADZONE_UP || RF2G4_Receive_Data[11] < JOYSTICK_DEADZONE_DOWN) ? 1 : 0; // 左摇杆左右
        uint8_t right_y = (RF2G4_Receive_Data[12] > JOYSTICK_DEADZONE_UP || RF2G4_Receive_Data[12] < JOYSTICK_DEADZONE_DOWN) ? 1 : 0; // 右摇杆上下
        uint8_t right_x = (RF2G4_Receive_Data[13] > JOYSTICK_DEADZONE_UP || RF2G4_Receive_Data[13] < JOYSTICK_DEADZONE_DOWN) ? 1 : 0; // 右摇杆左右

		//-----------------------------------------------

		for(int16_t i = 0; i < 2; i++)
		{
			// 船体运动控制模式（正常前进后退转向）
			
			// 左摇杆上下：左电机运动--测水深（水下）--云台上下
			if (RF2G4_Receive_Data[10] > JOYSTICK_DEADZONE_UP)
			{
				if (motor_start_flag == 1 && water_measure_flag == 0)			//01模式：控制左电机正转
				{	
					SHIP_RUN_LM(MOTOR_MAPPING(RF2G4_Receive_Data[10] - 127), MOTOR_foreward);
				}
				else if (motor_start_flag == 0 && water_measure_flag == 1)			//10模式：控制测水深电机正转，放绳
				{
					Motor_Get_water_depth(MOTOR_MAPPING(RF2G4_Receive_Data[10] - 127), 0);
					Water_depth_val += 0.02;						
				}
				else if(motor_start_flag == 1 && water_measure_flag == 1 && RF2G4_Receive_Data[10] == 255)			//11模式：控制二维云台垂直方向，上
				{
					Servo_Control_Advanced(RF2G4_Receive_Data[10],SERVO_vertical,0);
				}

			}
			else if(RF2G4_Receive_Data[10] < JOYSTICK_DEADZONE_DOWN)
			{
				if (motor_start_flag == 1 && water_measure_flag == 0)			//01模式：控制左电机反转
				{	
					SHIP_RUN_LM(MOTOR_MAPPING(RF2G4_Receive_Data[10] - 127), MOTOR_reversal);
				}
				else if (motor_start_flag == 0 && water_measure_flag == 1)			//10模式：控制测水深电机反转，收绳
				{
					if(Water_depth_val > 0)
					{
						Motor_Get_water_depth(MOTOR_MAPPING(RF2G4_Receive_Data[10] - 127), 1);
						Water_depth_val -= 0.02;					
					}

				}
				else if(motor_start_flag == 1 && water_measure_flag == 1 && RF2G4_Receive_Data[10] == 0)			//11模式：控制二维云台垂直方向，下
				{
					Servo_Control_Advanced(RF2G4_Receive_Data[10],SERVO_vertical,0);
				}
			}
			else
			{
				
			}

			// 左摇杆左右：无--无--云台左右
			if (RF2G4_Receive_Data[11] > JOYSTICK_DEADZONE_UP)
			{
				if (motor_start_flag == 1 && water_measure_flag == 0)			//01模式：
				{	

				}
				else if (motor_start_flag == 0 && water_measure_flag == 1)			//10模式：
				{
					
				}
				else if(motor_start_flag == 1 && water_measure_flag == 1 && RF2G4_Receive_Data[11] == 255)			//11模式：控制二维云台水平方向，左
				{
					Servo_Control_Advanced(RF2G4_Receive_Data[11],SERVO_horizontal,0);
				}
			}
			else if(RF2G4_Receive_Data[11] < JOYSTICK_DEADZONE_DOWN)
			{
				if (motor_start_flag == 1 && water_measure_flag == 0)			//01模式：
				{	

				}
				else if (motor_start_flag == 0 && water_measure_flag == 1)			//10模式：
				{
					
				}	
				else if(motor_start_flag == 1 && water_measure_flag == 1 && RF2G4_Receive_Data[11] == 0)			//11模式：控制二维云台水平方向，右
				{
					Servo_Control_Advanced(RF2G4_Receive_Data[11],SERVO_horizontal,0);
				}	
			}
			else
			{

			}


			// 右摇杆上下：右电机运动--采水样--测水深（水上）
			if (RF2G4_Receive_Data[12] > JOYSTICK_DEADZONE_UP)
			{
				if (motor_start_flag == 1 && water_measure_flag == 0)			//01模式：控制右电机正转
				{	
					SHIP_RUN_RM(MOTOR_MAPPING(RF2G4_Receive_Data[12] - 127), MOTOR_foreward);
				}
				else if (motor_start_flag == 0 && water_measure_flag == 1)			//10模式：控制采集水样电机抽水
				{
					if(Water_sample_val < 100)
					{
						Motor_Get_water_sample(MOTOR_MAPPING(RF2G4_Receive_Data[12] - 127), 0);	
						Water_sample_val += 2;
					}	

				}
				else if(motor_start_flag == 1 && water_measure_flag == 1)			//11模式：控制测水深电机正转，放绳
				{
					Motor_Get_water_depth(MOTOR_MAPPING(RF2G4_Receive_Data[12] - 127), 0);
				}
			}
			else if(RF2G4_Receive_Data[12] < JOYSTICK_DEADZONE_DOWN)
			{
				if (motor_start_flag == 1 && water_measure_flag == 0)			//01模式：控制右电机反转
				{	
					SHIP_RUN_RM(MOTOR_MAPPING(RF2G4_Receive_Data[12] - 127), MOTOR_reversal);
				}
				else if (motor_start_flag == 0 && water_measure_flag == 1)			//10模式：控制采集水样电机抽水
				{
					if(Water_sample_val > 0)
					{
						Motor_Get_water_sample(MOTOR_MAPPING(RF2G4_Receive_Data[12] - 127), 1);	
						Water_sample_val -= 2;
					}
					
				}
				else if(motor_start_flag == 1 && water_measure_flag == 1)			//11模式：控制测水深电机反转，收绳
				{
					Motor_Get_water_depth(MOTOR_MAPPING(RF2G4_Receive_Data[12] - 127), 1);
				}
			}
			else
			{

			}

			// 右摇杆左右：无--无--无
			if (RF2G4_Receive_Data[13] > JOYSTICK_DEADZONE_UP)
			{
				if (motor_start_flag == 1 && water_measure_flag == 0)			//01模式：
				{	

				}
				else if (motor_start_flag == 0 && water_measure_flag == 1)			//01模式：
				{
				
				}
				else if(motor_start_flag == 1 && water_measure_flag == 1)			//11模式：
				{
					
				}
			}
			else if(RF2G4_Receive_Data[13] < JOYSTICK_DEADZONE_DOWN)
			{
				if (motor_start_flag == 1 && water_measure_flag == 0)			//01模式：
				{	

				}
				else if (motor_start_flag == 0 && water_measure_flag == 1)			//01模式：
				{
					
				}
				else if(motor_start_flag == 1 && water_measure_flag == 1)			//11模式：
				{
					
				}
			}
			else
			{

			}

			if(left_y == 0 && left_x == 0 && right_y == 0 && right_x == 0)
			{
				if (motor_start_flag == 1 && water_measure_flag == 0)
				{	
					Motor_stop(1);
				}
				else if (motor_start_flag == 0 && water_measure_flag == 1)
				{
					Motor_stop(0);
				}
				else if (motor_start_flag == 1 && water_measure_flag == 1)
				{
					Motor_stop(0);
				}
			}

		}
		//-----------------------------------------------
			
        // 可选：处理其他按键事件（A/B/X/Y等）
        if (RF2G4_Receive_Data[0] == 1) /* X键(三角形)按下 */ 
		{ 
			if(motor_start_flag == 1 && water_measure_flag == 1)
			{
				if(move_started == 0) 
				{
					AUTO_MOVE_DIR = SHIP_FRONT;
					move_started = 1;
				}
				else if(move_started == 1) move_started = 0;
			}
			else
			{
				//PH
				ADC_dev_val.ADC_Water_target[0] += 0.5;
				//电导率
				ADC_dev_val.ADC_Water_target[1] += 50;
				//浊度
				ADC_dev_val.ADC_Water_target[2] += 15;
				//温度
				ADC_dev_val.ADC_Water_target[3] += 0;
			}
		}
        if (RF2G4_Receive_Data[1] == 1) /* B键(叉形)按下 */
		{ 
			if(motor_start_flag == 1 && water_measure_flag == 1)
			{
				
				if(move_started == 0) 
				{
					AUTO_MOVE_DIR = SHIP_BACK;
					move_started = 1;
				}
				else if(move_started == 1) move_started = 0;
			}
			else
			{
				//PH
				ADC_dev_val.ADC_Water_target[0] -= 0.5;
				//电导率
				ADC_dev_val.ADC_Water_target[1] -= 50;
				//浊度
				ADC_dev_val.ADC_Water_target[2] -= 15;
				//温度
				ADC_dev_val.ADC_Water_target[3] -= 0;
			}
		 }
        if (RF2G4_Receive_Data[2] == 1) /* A键(圆形)按下 */
		{ 
			if(motor_start_flag == 1 && water_measure_flag == 1)
			{
				
				if(move_started == 0) 
				{
					AUTO_MOVE_DIR = SHIP_RIGHT;
					AUTO_MOVE_TURN = 1;
					AUTO_MOVE_TURN_L = 2;
					move_started = 1;
				}
				else if(move_started == 1) move_started = 0;
			}
			else
			{
				//烟雾
				ADC_dev_val.ADC_Air_target[0] -= 5;
				//乙醇
				ADC_dev_val.ADC_Air_target[1] -= 0.25;
				//一氧化碳
				ADC_dev_val.ADC_Air_target[2] -= 1.5;
				//可燃气体
				ADC_dev_val.ADC_Air_target[3] -= 0.1;
			}
		 }
        if (RF2G4_Receive_Data[3] == 1) /* Y键(矩形)按下 */ 
		{ 
			if(motor_start_flag == 1 && water_measure_flag == 1)
			{
			
				if(move_started == 0) 
				{
					AUTO_MOVE_DIR = SHIP_LEFT;
					AUTO_MOVE_TURN = 1;
					AUTO_MOVE_TURN_L = 1;
					move_started = 1;
				}
				else if(move_started == 1) move_started = 0;
			}
			else
			{
				//烟雾
				ADC_dev_val.ADC_Air_target[0] += 5;
				//乙醇
				ADC_dev_val.ADC_Air_target[1] += 0.25;
				//一氧化碳
				ADC_dev_val.ADC_Air_target[2] += 1.5;
				//可燃气体
				ADC_dev_val.ADC_Air_target[3] += 0.1;
			}
		}
        if (RF2G4_Receive_Data[4] == 1) /* 左摇杆按下 */ 
		{ 
			
		}
        if (RF2G4_Receive_Data[5] == 1) /* 右摇杆按下 */ 
		{ 
			
		}

		//-----------------------------------------------	
		
	}	
	else//没收到消息，则显示no receive
	{
		//模拟看门狗---复位电机
		if(watch_dog_flag > 10)
		{
			Motor_stop(1);
			Motor_stop(0);			
		}
		watch_dog_flag++;  
		USART_printf(NRF24_DATA_HUART,"NRF no receive\r\n");
	}		
		
	//---------------------------------------------------
	if( F_2G4_Command_Valid == 0)// 每过0.5s判断判断命令是否失效,及如果命令在0.5s内没有更新，则判断为无效，可用于运行保险指令，防止小车出现失控
	{
			
	}

}

